/*
Copyright (c) (2012) (Anthony Bigot, Vincent Loppin)
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software
is furnished to do so, subject to the following conditions: The above copyright notice and
this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#ifndef MATRIX_H
#define MATRIX_H

namespace Util{

/*! \class Matrix<T>
  * \brief Manage Matrix 4x4
  * \author Anthony BIGOT & Vincent LOPPIN
  * \version 1.0
  *
  */
    template <class T>
    class Matrix{
        Matrix()
            {
                mat[0][0] = 0;	mat[0][1] = 0;	mat[0][2] = 0;	mat[0][3] = 0;
                mat[1][0] = 0;	mat[1][1] = 0;	mat[1][2] = 0;	mat[1][3] = 0;
                mat[2][0] = 0;	mat[2][1] = 0;	mat[2][2] = 0;	mat[2][3] = 0;
                mat[3][0] = 0;	mat[3][1] = 0;	mat[3][2] = 0;	mat[3][3] = 0;
            }

            Matrix(	T _a00, T _a01, T _a02, T _a03,
                    T _a10, T _a11, T _a12, T _a13,
                    T _a20, T _a21, T _a22, T _a23,
                    T _a30, T _a31, T _a32, T _a33)
            {
                mat[0][0] = _a00;	mat[0][1] = _a01;	mat[0][2] = _a02;	mat[0][3] = _a03;
                mat[1][0] = _a10;	mat[1][1] = _a11;	mat[1][2] = _a12;	mat[1][3] = _a13;
                mat[2][0] = _a20;	mat[2][1] = _a21;	mat[2][2] = _a22;	mat[2][3] = _a23;
                mat[3][0] = _a30;	mat[3][1] = _a31;	mat[3][2] = _a32;	mat[3][3] = _a33;
            }

            /// get Ptr on Matrix
            inline T* getPtr(){ return (T*)mat; }

            inline T* getPtr() const{ return (T*)mat; }

            /// Matrix add
            inline const Matrix<T> operator + (const Matrix<T> & m) const
            {
                return  Matrix<T>(	mat[0][0] + m.mat[0][0],
                                    mat[0][1] + m.mat[0][1],
                                    mat[0][2] + m.mat[0][2],
                                    mat[0][3] + m.mat[0][3],
                                    mat[1][0] + m.mat[1][0],
                                    mat[1][1] + m.mat[1][1],
                                    mat[1][2] + m.mat[1][2],
                                    mat[1][3] + m.mat[1][3],
                                    mat[2][0] + m.mat[2][0],
                                    mat[2][1] + m.mat[2][1],
                                    mat[2][2] + m.mat[2][2],
                                    mat[2][3] + m.mat[2][3],
                                    mat[3][0] + m.mat[3][0],
                                    mat[3][1] + m.mat[3][1],
                                    mat[3][2] + m.mat[3][2],
                                    mat[3][3] + m.mat[3][3]);
            }

            /// Matrix substract
            inline const Matrix<T> operator - (const Matrix<T> & m) const
            {
                return  Matrix<T>(	mat[0][0] - m.mat[0][0],
                                    mat[0][1] - m.mat[0][1],
                                    mat[0][2] - m.mat[0][2],
                                    mat[0][3] - m.mat[0][3],
                                    mat[1][0] - m.mat[1][0],
                                    mat[1][1] - m.mat[1][1],
                                    mat[1][2] - m.mat[1][2],
                                    mat[1][3] - m.mat[1][3],
                                    mat[2][0] - m.mat[2][0],
                                    mat[2][1] - m.mat[2][1],
                                    mat[2][2] - m.mat[2][2],
                                    mat[2][3] - m.mat[2][3],
                                    mat[3][0] - m.mat[3][0],
                                    mat[3][1] - m.mat[3][1],
                                    mat[3][2] - m.mat[3][2],
                                    mat[3][3] - m.mat[3][3]);
            }

            /// Product with a scalar
            inline const Matrix<T> operator * (const T sca) const
            {
                return  Matrix<T>(	mat[0][0]*sca,mat[0][1]*sca,mat[0][2]*sca,mat[0][3]*sca,
                                    mat[1][0]*sca,mat[1][1]*sca,mat[1][2]*sca,mat[1][3]*sca,
                                    mat[2][0]*sca,mat[2][1]*sca,mat[2][2]*sca,mat[2][3]*sca,
                                    mat[3][0]*sca,mat[3][1]*sca,mat[3][2]*sca,mat[3][3]*sca);
            }

            /// Divide by a scalar
            inline const Matrix<T> operator / (const T sca) const
            {
                return  Matrix<T>(	mat[0][0]/sca,mat[0][1]/sca,mat[0][2]/sca,mat[0][3]/sca,
                                    mat[1][0]/sca,mat[1][1]/sca,mat[1][2]/sca,mat[1][3]/sca,
                                    mat[2][0]/sca,mat[2][1]/sca,mat[2][2]/sca,mat[2][3]/sca,
                                    mat[3][0]/sca,mat[3][1]/sca,mat[3][2]/sca,mat[3][3]/sca);
            }

            /// Matrix product
            inline const Matrix<T> operator * (const Matrix<T> & m) const
            {
                Matrix<T> result;
                for(int i = 0; i < 4; ++i)
                {
                    for(int j = 0;j < 4; ++j)
                    {
                        for(int k = 0;k < 4;++k)
                        {
                            result.mat[i][j] +=	mat[i][k] * m.mat[k][j];
                        }
                    }
                }
                return result;
            }

            /// Matrix*Vec3 product (last coord is 1 (Vec3 is assimilate to be a point)
            inline const Vec3<T> operator * (const Vec3<T> & vec) const
            {
                Vec4<T> tmp(mat[0][0]*vec.x() + mat[0][1]*vec.y() +  mat[0][2]*vec.z() +  mat[0][3],
                            mat[1][0]*vec.x() + mat[1][1]*vec.y() +  mat[1][2]*vec.z() +  mat[1][3],
                            mat[2][0]*vec.x() + mat[2][1]*vec.y() +  mat[2][2]*vec.z() +  mat[2][3],
                            mat[3][0]*vec.x() + mat[3][1]*vec.y() +  mat[3][2]*vec.z() +  mat[3][3]);
                return Vec3<T>(tmp.x(),tmp.y(),tmp.z());
            }

            /// Matrix*Vec4 product
            inline const Vec4<T> operator * (const Vec4<T> & vec) const
            {
                return Vec4<T>(	mat[0][0]*vec.x() + mat[0][1]*vec.y() +  mat[0][2]*vec.z() +  mat[0][3],
                                mat[1][0]*vec.x() + mat[1][1]*vec.y() +  mat[1][2]*vec.z() +  mat[1][3],
                                mat[2][0]*vec.x() + mat[2][1]*vec.y() +  mat[2][2]*vec.z() +  mat[2][3],
                                mat[3][0]*vec.x() + mat[3][1]*vec.y() +  mat[3][2]*vec.z() +  mat[3][3]);
            }

            inline const Matrix<T> operator = (const Matrix<T> & m)
            {
                if(this == &m) return *this;
                mat[0][0] = m.mat[0][0];	mat[0][1] = m.mat[0][1];
                mat[0][2] = m.mat[0][2];	mat[0][3] = m.mat[0][3];
                mat[1][0] = m.mat[1][0];	mat[1][1] = m.mat[1][1];
                mat[1][2] = m.mat[1][2];	mat[1][3] = m.mat[1][3];
                mat[2][0] = m.mat[2][0];	mat[2][1] = m.mat[2][1];
                mat[2][2] = m.mat[2][2];	mat[2][3] = m.mat[2][3];
                mat[3][0] = m.mat[3][0];	mat[3][1] = m.mat[3][1];
                mat[3][2] = m.mat[3][2];	mat[3][3] = m.mat[3][3];
                return *this;
            }

            /// display matrice in given ostream
            inline void print(std::ostream &o) const
            {
                o.precision(5);
                o << std::fixed;
                o << "[" << mat[0][0] << ";" << mat[0][1] << ";" << mat[0][2] << ";" << mat[0][3] << "]" << std::endl;
                o << "[" << mat[1][0] << ";" << mat[1][1] << ";" << mat[1][2] << ";" << mat[1][3] << "]" << std::endl;
                o << "[" << mat[2][0] << ";" << mat[2][1] << ";" << mat[2][2] << ";" << mat[2][3] << "]" << std::endl;
                o << "[" << mat[3][0] << ";" << mat[3][1] << ";" << mat[3][2] << ";" << mat[3][3] << "]" << std::endl;
            }

            /// return identy matrix
            inline static Matrix<T> identity()
            {
                return Matrix<T>(	1,0,0,0,
                                    0,1,0,0,
                                    0,0,1,0,
                                    0,0,0,1	);
            }

            inline static Matrix<T> translation(T x, T y, T z)
            {
                return Matrix<T>(	1,0,0,0,
                                    0,1,0,0,
                                    0,0,1,0,
                                    x,y,z,1);
            }
            inline static Matrix<T> translation(const Vec3<T> & v)
            {
                return translation(v.x(),v.y(),v.z());
            }

            inline Vec3<T> getTranslation() const
            {
                return Vec3<T>(mat[3][0],mat[3][1],mat[3][2]);
            }

        protected:
            T mat[4][4];
    };

    template<typename T>
    std::ostream & operator<< (std::ostream & o, const Matrix<T> & m)
    {
        o.precision(5);
        o << std::fixed;
        o << "[" << m.getPtr()[0] << ";" << m.getPtr()[1] << ";" << m.getPtr()[2] << ";" << m.getPtr()[3] << "]" << std::endl;
        o << "[" << m.getPtr()[4] << ";" << m.getPtr()[5] << ";" << m.getPtr()[6] << ";" << m.getPtr()[7] << "]" << std::endl;
        o << "[" << m.getPtr()[ 8] << ";" << m.getPtr()[ 9] << ";" << m.getPtr()[10] << ";" << m.getPtr()[11] << "]" << std::endl;
        o << "[" << m.getPtr()[12] << ";" << m.getPtr()[13] << ";" << m.getPtr()[14] << ";" << m.getPtr()[15] << "]" << std::endl;
        return o;
    }
}

typedef Util::Matrix<double>        Matrixd;
typedef Util::Matrix<int>           Matrixi;
typedef Util::Matrix<unsigned int>	Matrixui;
typedef Util::Matrix<float>         Matrixf;
typedef Util::Matrix<unsigned char>	Matrixuc;

#endif // MATRIX_H
